#include "ros/ros.h"
#include <tf/tf.h>
#include "geometry_msgs/Twist.h"
#include "nav_msgs/Odometry.h"
#include <stdio.h>
#include <thread>
#include <unistd.h>
#include <string> 

//////////////////////////////////////////////////////////////////////////////
using std::string;

double Pi = 3.14159265;
double yaw, pitch, roll;
double Std_odom_x = 0, Std_odom_w = 0, Std_vel_x = 0, Std_vel_w = 0;  // std_:standard value;
double Acl_odom_x = 0, Acl_odom_w = 0, Acl_vel_x = 0, Acl_vel_w = 0;  // acl_:actual value.
geometry_msgs::Twist init_vel, set_vel, Acl_cmd_vel;  
double odom_Lx[50]={0}, odom_Ly[50]={0}, odom_Lw[50]={0};  //缓存数据变量
int flag_writeExcel = 0;   //写入Excel的标志位
int flag_break = 0;
string Excel_name, Excel_address, Excel_file;
char *Excel_file_;
//文件初始化，标题写入
void writeExcelinit()
{
    //标题名
	char* odom_x = "odom_x";
	char* odom_y = "odom_y";
	char* odom_w = "odom_w";
	
	FILE *fp = NULL;
	fp = fopen((const char*)Excel_file_,"w+");
	fprintf(fp,"  %s  \t  %s  \t  %s  \n",odom_x,odom_y,odom_w);
	fclose(fp);
}
void* writeExcel(double *odom_Lx, double *odom_Ly, double *odom_Lw, int n)
{
	FILE *fp = NULL;
	fp = fopen((const char*)Excel_file_,"a+");
	for (int i=0; i<n; i++)
    {
        fprintf(fp,"%lf\t%lf\t%lf\n",odom_Lx[i],odom_Ly[i],odom_Lw[i]);
    }
	fclose(fp);

    flag_writeExcel = 0;
}
//回调函数：返回里程计信息
void write_odom_callback (const nav_msgs::Odometry msg)
{
    // 四元数转欧拉角
    tf::Matrix3x3 mat(tf::Quaternion((msg.pose.pose.orientation.x), 
                                     (msg.pose.pose.orientation.y), 
                                     (msg.pose.pose.orientation.z), 
                                     (msg.pose.pose.orientation.w)));    
    
    mat.getEulerYPR(yaw, pitch, roll);

    if (set_vel.linear.x != 0)
    {
        Acl_odom_x = msg.pose.pose.position.x;
    }
    if (set_vel.angular.z != 0)
    {
        Acl_odom_w = yaw / Pi * 180;  //弧度值 转 角度值
    }

    static double last_odom_w = 0;
    if (Std_vel_w > 0)
    {
        if (Acl_odom_w < 0)
        {
            Acl_odom_w = last_odom_w + Acl_odom_w + Pi;
        }
        else 
        {
            Acl_odom_w = last_odom_w + Acl_odom_w;
        }
    
        last_odom_w = Acl_odom_w;
    }
    else if (Std_vel_w < 0)
    {

        if (Acl_odom_w > 0)
        {
            Acl_odom_w = last_odom_w + Acl_odom_w - Pi;
        }
        else 
        {
            Acl_odom_w = last_odom_w + Acl_odom_w;
        }
    
        last_odom_w = Acl_odom_w;
    }

    static int count_odom_buff = 0;
    odom_Lx[count_odom_buff] = Acl_odom_x;
    odom_Ly[count_odom_buff] = msg.pose.pose.position.y;
    odom_Lw[count_odom_buff] = Acl_odom_w;

    if(count_odom_buff >= 49)
    {
        flag_writeExcel = 1;
        count_odom_buff = 0;

        ROS_INFO_STREAM("it is writing:......");
    } 
    else 
    {
        count_odom_buff ++ ;
    }
}
//回调函数：返回里程计信息
void write_vel_callback (const geometry_msgs::Twist msg)
{
    Acl_cmd_vel.linear.x = msg.linear.x;
    Acl_cmd_vel.angular.z = msg.angular.z;

    if ((Acl_cmd_vel.linear.x == 0) && (Acl_cmd_vel.angular.z == 0))
    {
        flag_break = 1;
    }
}
///////////////////////////////////////////////////////////////////////////
int main (int argc, char** argv)
{
    //初始化节点
    ros::init(argc, argv, "excelodom");
    //声明节点句柄
    ros::NodeHandle nh_1;
    ros::NodeHandle nh_2;

    //订阅主题，并配置回调函数
    ros::Subscriber sub_odom = nh_1.subscribe("/odom",    1000, write_odom_callback);
    //订阅主题，并配置回调函数
    ros::Subscriber sub_vel  = nh_2.subscribe("/cmd_vel", 1000, write_vel_callback);

    //判断是否传入参数
    if(ros::param::has("~std_Vx"))
    {
        //传入参数赋值
       	ros::param::get("~std_Vx", Std_vel_x );
        ros::param::get("~std_Lx", Std_odom_x);

        set_vel.linear.x = Std_vel_x;
   	}
    else
    {
        set_vel.linear.x = 0.0;
    }   
       
    if(ros::param::has("~std_Vw"))
    {
       	ros::param::get("~std_Vw", Std_vel_w );
        ros::param::get("~std_Lw", Std_odom_w);

        set_vel.angular.z = Std_vel_w;
   	}
    else
    {
        set_vel.angular.z = 0.0;
    }

    ros::param::get("~Exl_name",    Excel_name);
    ros::param::get("~Exl_address", Excel_address);
    Excel_file  = Excel_address + Excel_name + ".csv";
    Excel_file_ = (char*)Excel_file.data();
    //初始化将写入的Excel文件
    writeExcelinit();

    ros::Rate loop_rate(20);                //循环周期：1 / x = _s; 1 / 20 = 0.05s = 50ms
    while (ros::ok())
    {
        ros::spinOnce();

        if (flag_writeExcel)
        {
            //定义一个线程
            std::thread t1(writeExcel, odom_Lx, odom_Ly, odom_Lw, 50);
            t1.join();
            // ROS_INFO_STREAM("正在写入：......");
        }
        if (flag_break)
        {
            flag_break = 0;
            //延时等待写入完成
            sleep(2);  //秒
            break;
        }
        loop_rate.sleep();
    }


}